#ifndef SEPARATION_OBS_H_ 
  #define SEPARATION_OBS_H_

#include <stdlib.h>
#include <iostream>
#include "../Vectores.h"
#include "../Obstacle.h"
#include "../Steering.h"
#include "../Kinematic.h"

using namespace std;

Steering* Obs_Sep(Steering* ret,vector<Obstacle *>* obstacle, Kinematic* character){
  float threshold = 100;
  float decayCoefficient = 6;
  float maxAcceleration = 10;
  float direction[3];
  float distance;
  float strength;
  unsigned int i;
  for ( i =0 ; i < obstacle->size();i++){
    restarVectores(character->position,(*obstacle)[i]->position,direction);
    distance = length(direction);
    if (distance < threshold){
      strength = min(decayCoefficient / (distance * distance), maxAcceleration);
      normalize(direction);
      multVectorScalar(direction,strength);
      sumarVectores(ret->linear,direction,ret->linear);
    }
  }
  return ret;
}
#endif
